#include "BeamQualityAnalysis.h"

BeamQualityAnalysis::BeamQualityAnalysis(QObject* parent) : QObject(parent) {
}

// 执行光束质量拟合的主函数
// 输入：测量位置、对应的光束宽度、激光波长
// 输出：包含拟合参数的结构体 BeamFittingParams
BeamFittingParams BeamQualityAnalysis::performBeamQualityFitting(const QVector<double>& positions,
                                                                 const QVector<double>& beamWidths,
                                                                 double wavelength) {
    emit analysisStarted();  // 发出分析开始信号

    BeamFittingParams params;
    params.wavelength = wavelength;  // 保存波长信息

    // 检查输入数据有效性：位置和光束宽度数量必须一致且不为空
    if (positions.size() != beamWidths.size() || positions.isEmpty()) {
        emit analysisError("位置和光束宽度数据不匹配");  // 发出错误信号
        return params;
    }

    try {
        emit analysisProgress(20);  // 更新进度为20%

        // 步骤1：找出光束腰位置（即光束最窄处的位置和宽度）
        double minWidth, waistPosition;
        findMinBeamWidthAndPosition(positions, beamWidths, minWidth, waistPosition);

        emit analysisProgress(40);  // 更新进度为40%

        // 步骤2：使用高斯-牛顿结合拉格朗日方法进行拟合
        QVector<double> fittingParams = gaussNewtonLagrange(positions, beamWidths);

        if (fittingParams.size() >= 3) {
            double c = fittingParams[0];  // 二次项系数
            double b = fittingParams[1];  // 一次项系数
            double a = fittingParams[2];  // 常数项

            emit analysisProgress(60);  // 更新进度为60%
            double discriminant = 0.0;
            // 步骤3：根据拟合结果计算光束质量相关参数
            if (wavelength == 1550e-6) {
                // 针对1550nm波长的特殊处理
                params.z0 = waistPosition;           // 光束腰位置
                params.w0 = minWidth / 2.0;          // 光束腰半径
                params.zr = (M_PI * pow(params.w0, 2)) / wavelength;  // 瑞利长度
            } else {
                // 通用情况下的处理
                params.z0 = -b / (2.0 * c);  // 拟合抛物线顶点（光束腰位置估计）
                discriminant = 4 * a * c - b * b;  // 判别式
                if (discriminant > 0) {
                    // 判别式大于0，可正常计算光束参数
                    params.w0 = sqrt(discriminant / (4.0 * c));  // 光束腰半径
                    params.zr = sqrt(discriminant) / (2.0 * c);        // 瑞利长度
                } else {
                    // 判别式非正，使用测量的最小宽度作为替代
                    params.w0 = minWidth / 2.0;
                    params.zr = 1.0;
                }
            }

            emit analysisProgress(80);  // 更新进度为80%

            // 步骤4：计算光束质量因子 M² 和光束参数积 BPP
            params.m2 = calculateM2Factor(params.w0, params.zr, wavelength, discriminant);
            params.bpp = calculateBPP(wavelength, params.m2);

            // 步骤5：验证拟合结果是否合理
            params.isValid = validateFittingResults(params);

            emit analysisProgress(100);  // 更新进度为100%

            // 打印拟合结果用于调试
            qDebug() << "拟合结果：z0=" << params.z0 << "mm, w0=" << params.w0
                     << "mm, zr=" << params.zr << "mm, M²=" << params.m2;
        }
    }
    catch (const std::exception& e) {
        // 捕获并处理可能的计算异常
        emit analysisError(QString("拟合计算错误: %1").arg(e.what()));
    }

    emit analysisCompleted();  // 发出分析完成信号
    return params;
}

// 高斯-牛顿结合拉格朗日方法的拟合函数（简化实现）
// 目标函数形式：y^2 = a*x^2 + b*x + c
// 输入：x坐标数组，y值数组，最大迭代次数，收敛容差（本实现中未使用）
// 输出：拟合参数 [a, b, c]
QVector<double> BeamQualityAnalysis::gaussNewtonLagrange(const QVector<double>& x,
                                                        const QVector<double>& y,
                                                        int maxIter, double tolerance) {
    // 注释说明：此为基于用户提供算法的实现
    QVector<double> params(3, 0.0);  // 初始化拟合参数 [a, b, c]

    // 检查输入数据有效性
    if (x.size() != y.size() || x.isEmpty()) {
        return params;
    }

    int n = x.size();

    // 初始化用于最小二乘拟合的矩阵 A 和向量 b
    QVector<QVector<double>> A(3, QVector<double>(3, 0.0));  // 3x3 矩阵
    QVector<double> b(3, 0.0);                               // 3x1 向量

    for (int i = 0; i < n; ++i) {
        double xi = x[i];
        double yi_sq = y[i] * y[i];  // y的平方，对应目标函数中的 y^2

        // 权重设置，避免除以零，这里使用 1/(4*y^2 + ε)
        double weight = 1.0 / (4.0 * y[i] * y[i] + 1e-10);

        // 当前数据点的雅可比矩阵行：[x^2, x, 1]
        QVector<double> jacobian = {xi * xi, xi, 1.0};

        // 构建正规方程：A^T * W * A * params = A^T * W * y^2
        for (int j = 0; j < 3; ++j) {
            for (int k = 0; k < 3; ++k) {
                A[j][k] += jacobian[j] * jacobian[k] * weight;
            }
            b[j] += jacobian[j] * yi_sq * weight;
        }
    }

    // 求解 3x3 的线性方程组（采用行列式法 / 克拉默法则）
    double det = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) -
                 A[0][1] * (A[1][0] * A[2][2] - A[1][2] * A[2][0]) +
                 A[0][2] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]);

    if (abs(det) > 1e-10) {  // 行列式非零，方程组有唯一解
        // 使用克拉默法则分别求解 a, b, c
        params[0] = (b[0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) -
                     b[1] * (A[0][1] * A[2][2] - A[0][2] * A[2][1]) +
                     b[2] * (A[0][1] * A[1][2] - A[0][2] * A[1][1])) / det;

        params[1] = (A[0][0] * (b[1] * A[2][2] - b[2] * A[1][2]) -
                     A[0][1] * (b[0] * A[2][2] - b[2] * A[2][0]) +
                     A[0][2] * (b[0] * A[1][2] - b[1] * A[2][0])) / det;

        params[2] = (A[0][0] * (A[1][1] * b[2] - A[1][2] * b[1]) -
                     A[0][1] * (A[1][0] * b[2] - A[1][2] * b[0]) +
                     A[0][2] * (A[1][0] * b[1] - A[1][1] * b[0])) / det;
    }

    return params;  // 返回拟合得到的参数 [a, b, c]
}

QVector<double> BeamQualityAnalysis::generateMeasurementPositions(const BeamFittingParams& paramsX,
                                                                 const BeamFittingParams& paramsY,
                                                                 const ScanConfiguration& scanConfig) {
    // 基于用户的CalculatePositions算法
    return calculateOptimalPositions(paramsX.z0, paramsX.zr, 0.0);  // 假设焦距为0或从配置获取
}


QVector<double> BeamQualityAnalysis::calculateOptimalPositions(double z0, double zr, double zf) {
    // 基于用户提供的算法实现
    QVector<double> positions(21);

    const int centerIndex = 10;
    positions[centerIndex] = z0;  // 光束腰位置

    // 在光束腰附近分布点位
    double d1 = zr * 1.4 / 7.0;
    for (int i = 1; i <= 7; ++i) {
        positions[centerIndex - i] = z0 - d1 * i;
        positions[centerIndex + i] = z0 + d1 * i;
    }

    // 远场点位
    double d2 = zr * 0.8;
    positions[3] = positions[4] - d2;
    positions[17] = positions[16] + d2;

    double d3 = zr * 1.2;
    positions[2] = positions[3] - d3 * 0.3;
    positions[1] = positions[3] - d3 * 0.7;
    positions[0] = positions[3] - d3;

    positions[18] = positions[17] + d3 * 0.3;
    positions[19] = positions[17] + d3 * 0.7;
    positions[20] = positions[17] + d3;

    return positions;
}

void BeamQualityAnalysis::findMinBeamWidthAndPosition(const QVector<double>& positions,
                                                     const QVector<double>& beamWidths,
                                                     double& minWidth, double& correspondingPosition) {
    if (positions.isEmpty() || beamWidths.isEmpty()) {
        minWidth = 0.0;
        correspondingPosition = 0.0;
        return;
    }

    minWidth = beamWidths[0];
    correspondingPosition = positions[0];

    for (int i = 1; i < beamWidths.size(); ++i) {
        if (beamWidths[i] < minWidth) {
            minWidth = beamWidths[i];
            correspondingPosition = positions[i];
        }
    }
}

double BeamQualityAnalysis::calculateM2Factor(double w0, double zr, double wavelength, double discriminant) {
    if (w0 <= 0 || zr <= 0 || wavelength <= 0) return 1.0;
    return M_PI/(8*wavelength )*sqrt(discriminant);
}

double BeamQualityAnalysis::calculateBPP(double wavelength, double m2) {
    return (wavelength * m2 / M_PI) * 1000.0;  // 转换为 mm·mrad
}

bool BeamQualityAnalysis::validateFittingResults(const BeamFittingParams& params) {
    return params.w0 > 0 && params.zr > 0 && params.m2 > 0;
}

QVector<ScanDataPoint> BeamQualityAnalysis::filterScanData(const QVector<ScanDataPoint>& rawData) {
    QVector<ScanDataPoint> filteredData;

    for (const auto& point : rawData) {
        // 过滤掉异常数据点
        if (point.beamWidthX > 0 && point.beamWidthY > 0 &&
            point.beamWidthX < 100.0 && point.beamWidthY < 100.0) {  // 假设最大光束宽度100mm
            filteredData.append(point);
        }
    }

    return filteredData;
}

BeamQualityMeasurementResults BeamQualityAnalysis::calculateFullResults(const QVector<ScanDataPoint>& scanData,
                                                                       const ScanConfiguration& config) {
    BeamQualityMeasurementResults results;
    results.scanData = filterScanData(scanData);

    if (results.scanData.isEmpty()) {
        emit analysisError("没有有效的扫描数据");
        return results;
    }

    // 提取位置和光束宽度数据
    QVector<double> positions, beamWidthsX, beamWidthsY;
    for (const auto& point : results.scanData) {
        positions.append(point.position);
        beamWidthsX.append(point.beamWidthX);
        beamWidthsY.append(point.beamWidthY);
    }

    // 分别拟合X和Y方向
    results.paramsX = performBeamQualityFitting(positions, beamWidthsX);
    results.paramsY = performBeamQualityFitting(positions, beamWidthsY);

    // 生成测量点位
    results.measurementPositions = generateMeasurementPositions(results.paramsX, results.paramsY, config);

    return results;
}


void  BeamQualityAnalysis::setWavelength(double wavelength) {
    m_wavelength = wavelength;
}